import matplotlib.pyplot as plt
import numpy as np
from adrc_fhan import fhan

MaxSpeed = 1000  # 最大速度(步数/s)
MaxAcc = 1000  # 最大加速度(步数/s^2)
PulseTarget = 1500  # 跟踪微分器需要跟踪的目标脉冲数
h = 0.02  # 控制周期(s)
N = 7  # 仿真时长(s)
period = 1e-5  # 定时器周期(s)
frequency = 1e5  # 定时器频率(Hz)
ctrlP = int(h / period + 0.5)  # 几个定时器周期输出一次控制信号
LENTMR = int(N / period + 0.5)  # 定时器溢出总次数
LENCTRL = int(N / h + 0.5)  # 控制信号输出总次数
TimerPreviousCnt = 0
CtrlCnt = 0  # 控制信号输出计数
MotorPosition = np.zeros(LENTMR)  # 步进电机角度
# MotorSpeed = np.zeros(LENTMR)  # 步进电机角速度
tplot = np.zeros(LENCTRL)  # 绘图横坐标变量
xplot1 = np.zeros(LENCTRL)  # 绘图纵坐标变量
xplot2 = np.zeros(LENCTRL)  # 绘图纵坐标变量
x1 = 0  # 跟踪微分器位置输出
x2 = 0  # 跟踪微分器速度输出
u = 0  # 跟踪微分器加速度输出
width = 400  # 实际脉宽
direct = 0
err = 0
t = 0
for n in range(LENTMR):
    t = n * period  # 全局时间
    if t > 1.5:
        PulseTarget = -1000

    if n % ctrlP == 0:
        u = fhan(MotorPosition[n] - PulseTarget, x2, h, MaxAcc, MaxSpeed)
        x1 += h * x2
        x2 += h * u
        if abs(x2) < 3.06 or MotorPosition[n] == PulseTarget:
            direct = 0
            x2 = 0
        else:
            width = abs(frequency / x2)
            if x2 > 0:
                direct = 1
            else:
                direct = -1
        xplot1[CtrlCnt] = x1
        xplot2[CtrlCnt] = x2*np.pi/100
        tplot[CtrlCnt] = CtrlCnt * h
        CtrlCnt = CtrlCnt + 1

    if n == LENTMR-1:
        break

    if n >= TimerPreviousCnt + int(width):
        MotorPosition[n + 1] = MotorPosition[n] + direct
        TimerPreviousCnt = n
    else:
        MotorPosition[n + 1] = MotorPosition[n]

params = {"font.family":'serif', "font.size": 20, "mathtext.fontset":'stix', "font.serif": ['SimSun'],}
plt.rcParams.update(params)
fig, ax = plt.subplots()
ax.plot(tplot, xplot2, color='black', linewidth=1)
plt.subplots_adjust(top=0.99, bottom=0.14, left=0.14, right=0.99)
plt.xlabel(r'时间t(s)')
plt.ylabel(r'转速$\omega$(rad/s)')
plt.show()